#include "StdAfx.h"
#include <math.h>
#include <stdlib.h>
#include ".\simmanager.h"

#define MAX_SELECT_ANNEAL 20.0

SimManager::SimManager(void):m_nNumObstacles(0)
{
	m_bQlearnOnly = m_bBoostWithQlearn = false;
	Pos2d p2Min;
	Pos2d p2Max;
	p2Min[0] = p2Min[1] = p2Max[0] = p2Max[1] = 0.0;
	SetTrainingObstacleParams(0, 3, p2Min, p2Max, 5.0);
	SetGoal(25.0, 25.0, 6.0);	
}

SimManager::~SimManager(void)
{
}

void SimManager::RunRangeFinders()
{
	
	for(int iObstacle = 0; iObstacle < m_nNumObstacles; ++iObstacle)
	{
		m_Collider.SetRaysForConvexPoly(m_arrObstacles[iObstacle].GetPolyPts(), 
			m_arrObstacles[iObstacle].GetNumPts());
	}
}

bool SimManager::RunRobotOneStep(bool & bHitGoal)
{
	Pos2d p2ToGoal;
	
	RunRangeFinders();
	m_Controller.SetRangeSensorData(-1, m_Collider.RangeFinders().GetRangeArr());
	m_Collider.GetGoalFromFront(m_p2Goal, p2ToGoal);
	m_Controller.SetGoalData(-1, p2ToGoal[0], p2ToGoal[1]);
	m_Controller.SetInternalSensors(-1);

	m_Controller.GetRewardsFromState(-1);

	
	int nAction = m_Controller.SelectActionProbabalistically(0.0, MAX_SELECT_ANNEAL);

	m_lfCurrGoodness = m_Controller.GetActionReward(nAction);
	
	m_Controller.UpdateStateFromAction(nAction);

	bool bMoveValid = MoveRobot(m_Controller.GetTurnRate(), m_Controller.GetSpeed());

	bHitGoal = m_Collider.CollideSphere(m_p2Goal, m_lfGoalRad);
#if _DEBUG
	if(bHitGoal)
	{
		// fprintf(stderr,"Hit goal (or so I think)\n");
		int nDoStuff = 5;
		nDoStuff += 2;
	}
#endif
	RunRangeFinders();
	return bMoveValid && !bHitGoal;
}

void SimManager::SetTrainingObstacleParams(int nNumObstacles, int nNumSides, Pos2d p2ObstacleMin, Pos2d p2ObstacleMax, double lfObstacleRad)
{
	m_nNumObstacles = nNumObstacles;
	m_nNumSides = nNumSides;
	m_p2ObstacleMin[0] = p2ObstacleMin[0];
	m_p2ObstacleMin[1] = p2ObstacleMin[1];
	m_p2ObstacleMax[0] = p2ObstacleMax[0];
	m_p2ObstacleMax[1] = p2ObstacleMax[1];
	m_lfObstacleRad = lfObstacleRad;
}

void SimManager::PlaceRobotAndGoal()
{
	double lfGoalX = ((rand()/((double)RAND_MAX))-0.5)*150.0;
	double lfGoalY = ((rand()/((double)RAND_MAX))-0.5)*150.0;

	double lfRobotX = ((rand()/((double)RAND_MAX))-0.5)*150.0;
	double lfRobotY = ((rand()/((double)RAND_MAX))-0.5)*150.0;

	while(fabs(lfGoalY-lfRobotY) < m_lfGoalRad+15.0 ||
		fabs(lfGoalX-lfRobotY) < m_lfGoalRad+15.0)
	{
		lfGoalX = ((rand()/((double)RAND_MAX))-0.5)*50.0;
		lfGoalY = ((rand()/((double)RAND_MAX))-0.5)*50.0;
	}

	double lfRobotAngle = 3.1415926535897931*(rand()/((double)RAND_MAX));

	SetGoal(lfGoalX, lfGoalY, GetGoalRad());
	m_Collider.SetPosition(lfRobotX, lfRobotY, lfRobotAngle);
}

void SimManager::Train(int nTimes, const char * szFileOut, int nEvalFreq, double lfLearningRate, double lfDiscountFactor)
{
	if(szFileOut[0] == '\0')
	{
		return;
	}
	FILE * fiOut = fopen(szFileOut, "w");

	m_nHitGoalCount = m_nHitObstacleCount = m_nMissedGoalCount=0;
	for(int i = 0; i < nTimes; ++i)
	{

		double lfDistWindow = 150.0; //20.0+(50.0*i)/nTimes;
		double lfGoalX = ((rand()/((double)RAND_MAX))-0.5)*lfDistWindow;
		double lfGoalY = ((rand()/((double)RAND_MAX))-0.5)*lfDistWindow;

		double lfRobotX = ((rand()/((double)RAND_MAX))-0.5)*lfDistWindow;
		double lfRobotY = ((rand()/((double)RAND_MAX))-0.5)*lfDistWindow;

		while(fabs(lfGoalY-lfRobotY) < m_lfGoalRad+5.0 ||
			fabs(lfGoalX-lfRobotY) < m_lfGoalRad+5.0)
		{
			lfGoalX = ((rand()/((double)RAND_MAX))-0.5)*lfDistWindow;
			lfGoalY = ((rand()/((double)RAND_MAX))-0.5)*lfDistWindow;
		}

		double lfRobotAngle = 3.1415926535897931*(rand()/((double)RAND_MAX));

		SetGoal(lfGoalX, lfGoalY, GetGoalRad());
		m_Collider.SetPosition(lfRobotX, lfRobotY, lfRobotAngle);

		if(i%nEvalFreq || nEvalFreq < 0)
		{
			TrainOnce(m_lfAnnealMagic/(m_lfAnnealMagic+i), 
				MAX_SELECT_ANNEAL-(m_lfAnnealMagic*(MAX_SELECT_ANNEAL-1.0))/(m_lfAnnealMagic+i), 
				0.6, lfLearningRate, lfDiscountFactor);
		}
		else
		{
			if(nEvalFreq > 20)
			{
				char szFileName[80];
				sprintf(szFileName, "C:\\nnet_backup\\nnet_%d_default.nnet", i);
				m_Controller.SaveNeuralNet(szFileName);
			}
			EvalOnce(fiOut);
			fflush(fiOut);
		}
	}
	fclose(fiOut);
}


void SimManager::EvalOnce(FILE * fiOut)
{
	m_Controller.ZeroInternalState();
	bool bMoveValid = true;
	bool bHitGoal = false;
	// bool bOutOfRange = false;
	
	Pos2d p2ToGoalInitial;
	m_Collider.GetGoalFromFront(m_p2Goal, p2ToGoalInitial);
	double lfDistSqrdToGoal = 0.0;
	int nIter = 0;
	for(; (lfDistSqrdToGoal < 45000.0) && !bHitGoal && bMoveValid && nIter < m_Controller.GetMaxIter(); ++nIter)
	{
		Pos2d p2ToGoal;	
		m_Collider.SetRangefinderRange(30.0);
		RunRangeFinders();
		m_Controller.SetRangeSensorData(nIter, m_Collider.RangeFinders().GetRangeArr());
		m_Collider.GetGoalFromFront(m_p2Goal, p2ToGoal);
		m_Controller.SetGoalData(nIter, p2ToGoal[0], p2ToGoal[1]);
		m_Controller.SetInternalSensors(nIter);
		m_Controller.GetRewardsFromState(nIter);
		int nAction = m_Controller.SelectActionProbabalistically(0.0, MAX_SELECT_ANNEAL);
		// int nAction = m_Controller.SelectMaxAction();
		m_Controller.UpdateStateFromAction(nAction);

		// m_Controller.SetActionForTraining(nIter, nAction);

		bMoveValid = MoveRobot(m_Controller.GetTurnRate(), m_Controller.GetSpeed());
		
		lfDistSqrdToGoal = p2ToGoal[0]*p2ToGoal[0]+p2ToGoal[1]*p2ToGoal[1];
		// bMoveValid = bMoveValid && (lfDistSqrdToGoal < 10000.0);
		bHitGoal = m_Collider.CollideSphere(m_p2Goal, m_lfGoalRad);
	}

	if(bHitGoal)
	{
		++m_nHitGoalCount;
	}
	else if(!bMoveValid)
	{
		++m_nHitObstacleCount;
	}
	else
	{
		++m_nMissedGoalCount;	
	}
	fprintf(fiOut, "%d,%d,%d,%lf,%lf,%lf, %lf\n", bMoveValid, bHitGoal, nIter,
		lfDistSqrdToGoal, p2ToGoalInitial[0], p2ToGoalInitial[1], 
		sqrt(p2ToGoalInitial[0]*p2ToGoalInitial[0]+p2ToGoalInitial[1]*p2ToGoalInitial[1]));
}



void SimManager::TrainOnce(double lfActionSelectionBias, double lfMaxSelectionBias, double lfCollisionPunishment, 
						   double lfTrainRate, double lfDiscountFactor)
{
	m_Controller.ZeroInternalState();
	bool bMoveValid = true;
	bool bHitGoal = false;
	// bool bOutOfRange = false;
	
	double lfFinalReward;

	double lfLogDiscount = log(lfDiscountFactor);

	double lfDistSqrdToGoal = 0.0;
	int nIter = 0;
	int nMaxTrainingIter = 200;
	//if(m_bQlearnOnly)
	//{
	//	nMaxTrainingIter = (rand())%(m_Controller.GetMaxIter()-5)+5;
	//}
	nMaxTrainingIter = m_Controller.GetMaxIter(); // (rand())%(m_Controller.GetMaxIter()-100)+50; // sorry, more magic numbers
	if(m_bQlearnOnly)
	{
		nMaxTrainingIter = (rand())%(m_Controller.GetMaxIter()-5)+5;
	}

	Pos2d p2ToGoal;

	for(; (lfDistSqrdToGoal < 45000.0) && !bHitGoal && bMoveValid && nIter < nMaxTrainingIter ; 
		++nIter)
	{
		
		m_Collider.SetRangefinderRange(30.0);
		RunRangeFinders();
		m_Controller.SetRangeSensorData(nIter, m_Collider.RangeFinders().GetRangeArr());
		m_Collider.GetGoalFromFront(m_p2Goal, p2ToGoal);
		m_Controller.SetGoalData(nIter, p2ToGoal[0], p2ToGoal[1]);
		m_Controller.SetInternalSensors(nIter);
		m_Controller.GetRewardsFromState(nIter);
		int nAction = m_Controller.SelectActionProbabalistically(lfActionSelectionBias, lfMaxSelectionBias);
		m_Controller.UpdateStateFromAction(nAction);

		m_Controller.SetActionForTraining(nIter, nAction);

		lfFinalReward = m_Controller.GetActionReward(nAction);

		bMoveValid = MoveRobot(m_Controller.GetTurnRate(), m_Controller.GetSpeed());
		
		lfDistSqrdToGoal = p2ToGoal[0]*p2ToGoal[0]+p2ToGoal[1]*p2ToGoal[1];
		// bMoveValid = bMoveValid && (lfDistSqrdToGoal < 10000.0);
		bHitGoal = m_Collider.CollideSphere(m_p2Goal, m_lfGoalRad);

		
	}
	double lfPayoff=lfCollisionPunishment;

	// just make it learn something easier for the time being -- take this out later
	//if(!bHitGoal)
	//{
	//	m_Collider.GetGoalFromFront(m_p2Goal, p2ToGoal);
	//	bHitGoal = (fabs(atan2(m_p2Goal[1], m_p2Goal[0])) < 0.2);
	//}
	////////////////////////////////////////////////////////////////////////////////

	if(bHitGoal)
	{
		lfPayoff = 1.0;
		++m_nHitGoalCount;
	}
	else if(!bMoveValid)
	{
		lfPayoff = 0.0; //-lfCollisionPunishment;
		++m_nHitObstacleCount;
	}
	else if(lfDistSqrdToGoal >= 45000.0)
	{
		lfPayoff = 0.0;
		++m_nMissedGoalCount;
	}
	else
	{
		++m_nMissedGoalCount;
		
		lfPayoff = lfFinalReward;
		//if(m_bQlearnOnly)
		//{
		//	lfPayoff = lfFinalReward;
		//}
		//else
		//{
		//	double lfRewardHeuristic = exp(lfLogDiscount*(m_Collider.DistanceToPoint(m_p2Goal)*2));
		//	//double lfDistBoost = (1.0-0.01*m_Collider.DistanceToPoint(m_p2Goal));
		//	//if(lfDistBoost < 0)
		//	//{
		//	//	lfDistBoost = 0.0;
		//	//}
		//	//lfPayoff += lfDistBoost*(0.9-lfPayoff);

		//	lfPayoff = lfRewardHeuristic*(1.0-lfCollisionPunishment)+lfCollisionPunishment;
		//}
		
	}



	if(m_bQlearnOnly)
	{
		if(bHitGoal)
		{
			m_Controller.Train(nIter, lfTrainRate, lfDiscountFactor, 1.0, 0.0);
		}
		m_Controller.TrainQLearning(nIter, lfTrainRate, lfDiscountFactor, lfPayoff);
		m_Controller.TrainQLearning(nIter, lfTrainRate, lfDiscountFactor, lfPayoff);
	}
	else
	{
		m_Controller.Train(nIter, lfTrainRate, lfDiscountFactor, lfPayoff, lfCollisionPunishment);
		if(m_bBoostWithQlearn)
		{
			m_Controller.TrainQLearning(nIter, lfTrainRate, lfDiscountFactor, lfPayoff);
		}
	}
}

bool SimManager::MoveRobot(int nTurnAmount, int nSpeed)
{
	m_Collider.Move(nTurnAmount, nSpeed);
	for(int iObstacle = 0; iObstacle < m_nNumObstacles; ++iObstacle)
	{
		if(m_Collider.HitsBbox(m_arrObstacles[iObstacle].GetBbox()))
		{
			if(m_Collider.CollideConvexPoly(m_arrObstacles[iObstacle].GetPolyPts(),
				m_arrObstacles[iObstacle].GetNumPts()))
			{
				m_Collider.Move(nTurnAmount, -nSpeed);
				return false;
			}
		}

	}


#if _DEBUG
	bool bHitGoal = m_Collider.CollideSphere(m_p2Goal, m_lfGoalRad);
	if(bHitGoal)
	{
		// fprintf(stderr,"Hit goal (or so I think)\n");
		int nDoStuff = 5;
		nDoStuff += 2;
	}
#endif
	return true;
}

void SimManager::SetAllObstacleParams(int nNumObstacles, int nNumSides, 
									  Pos2d p2ObstacleMin, Pos2d p2ObstacleMax, 
									  double lfObstacleRad)
{
	SetTrainingObstacleParams(nNumObstacles, nNumSides, p2ObstacleMin, p2ObstacleMax, lfObstacleRad);
	SetNumObstacles(nNumObstacles);
	SetObstacleNumPolys(nNumSides);
	for(int i = 0; i < m_nNumObstacles; ++i)
	{
		SetupObstacle(i, p2ObstacleMin, p2ObstacleMax, lfObstacleRad);
	}

	RunRangeFinders();
}


void SimManager::SetRobotParams(double lfLength, double lfWidth, double lfXPos, double lfYPos, double lfTheta, double lfAngleStep)
{

	m_Collider.SetParams(lfLength, lfWidth, lfXPos, lfYPos, lfTheta);
	m_Collider.SetAngleStep(lfAngleStep);
}

bool SimManager::SetObstacleNumPolys(int nNumPts)
{
	for(int i = 0; i < 8; ++i)
	{
		m_arrObstacles[i].SetPolySize(nNumPts);
	}
	return true;
}

bool SimManager::SetupObstacle(int iWhichObstacle, Pos2d p2ObstacleMin, Pos2d p2ObstacleMax, double lfObstacleRadius)
{
	if(iWhichObstacle >= m_nNumObstacles || iWhichObstacle < 0)
	{
		return false;
	}
	double arrLfGoalBbox[4];
	arrLfGoalBbox[0] = m_p2Goal[0] - m_lfGoalRad;
	arrLfGoalBbox[1] = m_p2Goal[1] - m_lfGoalRad;
	arrLfGoalBbox[2] = m_p2Goal[0] + m_lfGoalRad;
	arrLfGoalBbox[3] = m_p2Goal[1] + m_lfGoalRad;
	m_arrObstacles[iWhichObstacle].SetPolyPositionsRandom(p2ObstacleMin, p2ObstacleMax, lfObstacleRadius, m_Collider.GetBbox(), arrLfGoalBbox);
	return true;
}
